"""
  Copyright 2018 The Cartographer Authors
  Copyright 2022 Wyca Robotics (for the ros2 conversion)

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import Shutdown
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

def generate_launch_description():

    ## ***** Launch arguments *****
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')

    # ***** File paths ******
    #
    # pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    #新增
    
    urdf_pkg = get_package_share_directory(
        "turtlebot3_gazebo"
    )
    urdf_dir = os.path.join(urdf_pkg, 'urdf')
    urdf_file = os.path.join(urdf_dir, 'turtlebot3_zbrover.urdf')
    with open(urdf_file, 'r') as infp:
        robot_desc = infp.read()
    

    ## ***** Nodes *****
    #这个节点被注释了
    robot_state_publisher_node = Node(
        package = 'robot_state_publisher',
        executable = 'robot_state_publisher',
        parameters=[
            {'robot_description': robot_desc},
            # {'use_sim_time': LaunchConfiguration('use_sim_time')}
            ],
        output = 'screen'
        )
    #tf_s2和robot_state_publisher_node都是为了描述机器人的各个部件的坐标关系
    # tf_s2 = Node(
            # package='tf2_ros',         # tf2_ros 包
            # executable='static_transform_publisher',  # 执行静态坐标系发布器
            # name='static_transform_publisher',
            # output='screen',
            # arguments=['1.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'laser']  # 参数：位移(x, y, z), 旋转(rx, ry, rz), 父坐标系, 子坐标系
        # )
    joint_state_publisher = Node(
        package="joint_state_publisher",
        executable="joint_state_publisher"
        )       


    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 's2sllidar_odom_imu.lua'],   #这里的lua文件指定为自己写的那个
        remappings = [
            # ('echoes', 'horizontal_laser_2d')], 
            ('scan', '/scan'),
            ('odom', '/odom'),
            ("imu", "/imu"),
            ], #前面的为cartographer订阅的话题，是固定的ROS的API，分别为:scan;echoes;points2;imu;odom五个https://google-cartographer-ros.readthedocs.io/en/latest/ros_api.html
        output = 'screen'
        )

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': True},
            {'resolution': 0.05}],
        )
    
    # rviz_node = Node(
        # package = 'rviz2',
        # executable = 'rviz2',
        # on_exit = Shutdown(),
        # arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'],
        # parameters = [{'use_sim_time': True}],
    # )
     
    rviz2 = Node(
        package="rviz2",
        executable="rviz2",
        # arguments=["-d",get_package_share_directory("cpp06_urdf")+"/rviz/urdf.rviz"]    
    )
    
    return LaunchDescription([
        # use_sim_time_arg,
        # Nodes
        robot_state_publisher_node, 
        joint_state_publisher,
        # tf_s2,
        # cartographer_node,
        # cartographer_occupancy_grid_node,
        rviz2,
        # rviz_node,
    ])
